//
// Created by 刘洋 on 2024/2/2.
//
#include "AS5600.h"

#define abs(x) ((x)>0?(x):-(x))
#define _2PI 6.28318530718

static float angle_data_prev; //上次位置
static float full_rotation_offset; //转过的整圈数

static int32_t full_rotations;
static float angle_prev;
void bsp_as5600Init() {
    /* init i2c interface */

    /* init var */
    angle_prev = 0;
    full_rotations = 0;
    full_rotation_offset = 0;
    angle_data_prev = bsp_as5600GetRawAngle();
}

static int i2cWrite(uint8_t dev_addr, uint8_t *pData, uint32_t count) {
    int status;
    int i2c_time_out = I2C_TIME_OUT_BASE + count * I2C_TIME_OUT_BYTE;

    status = HAL_I2C_Master_Transmit(&AS5600_I2C_HANDLE, dev_addr, pData, count, i2c_time_out);
    return status;
}

static int i2cRead(uint8_t dev_addr, uint8_t *pData, uint32_t count) {
    int status;
    int i2c_time_out = I2C_TIME_OUT_BASE + count * I2C_TIME_OUT_BYTE;
    status = HAL_I2C_Master_Receive(&AS5600_I2C_HANDLE, (dev_addr | 1), pData, count, i2c_time_out);
    return status;
}

uint16_t bsp_as5600GetRawAngle() {
    uint16_t raw_angle;
    uint8_t buffer[2] = {0};
    uint8_t raw_angle_register = AS5600_RAW_ANGLE_REGISTER;

    i2cWrite(AS5600_ADDR, &raw_angle_register, 1);
    i2cRead(AS5600_ADDR, buffer, 2);
    raw_angle = ((uint16_t)buffer[0] << 8) | (uint16_t)buffer[1];
    return raw_angle;
}

float bsp_as5600GetAngle() {
    float angle_data = bsp_as5600GetRawAngle();

    float d_angle = angle_data - angle_data_prev;
    if(abs(d_angle) > (0.8 * AS5600_RESOLUTION)) {
        full_rotation_offset += (d_angle > 0 ? -_2PI : _2PI);
    }
    angle_data_prev = angle_data;

    return (full_rotation_offset + (angle_data / (float)AS5600_RESOLUTION)*_2PI);
}
float getAngle_Without_track()
{
    return bsp_as5600GetRawAngle()*0.08789* M_PI / 180.0f;    //得到弧度制的角度
}

float getAngle()
{
    float val = getAngle_Without_track();
    float d_angle = val - angle_prev;
    //计算旋转的总圈数
    //通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出，如果发生了，则将full_rotations增加1（如果d_angle小于0）或减少1（如果d_angle大于0）。
    if(abs(d_angle) > (0.8f*6.28318530718f) ) full_rotations += ( d_angle > 0 ) ? -1 : 1;
    angle_prev = val;
    return (float)full_rotations * 6.28318530718f + angle_prev;
}
